//
// Created by fazhehy on 2024/7/4.
//

#include "enu.h"

#define EARTH_R     6378137

gnss_point_t origin;
enu_point_t enu_target_point[50];
int16_t enu_target_point_len = 0;
enu_point_t enu_now_point;
gnss_point_t gnss_now_point;

void gnss_to_enu(gnss_point_t * gnss, enu_point_t * enu);

void enu_init()
{
    origin.latitude = gnss_target_point[0].latitude;
    origin.longitude = gnss_target_point[0].longitude;
    enu_target_point_len = gnss_target_point_len;
    enu_target_point[0].x = 0;
    enu_target_point[1].y = 0;
    for (int i = 1; i < enu_target_point_len; ++i) {
        gnss_to_enu(gnss_target_point+i, enu_target_point+i);
    }
}

void enu_get_now_point()
{
    if (gnss_flag == 1) {
        gnss_data_parse();
        gnss_now_point.latitude = gnss.latitude;
        gnss_now_point.longitude = gnss.longitude;
        gnss_to_enu(&gnss_now_point, &enu_now_point);
        gnss_flag = 0;
    }
}

void gnss_to_enu(gnss_point_t * gnss, enu_point_t * enu)
{
    enu->y = EARTH_R*(ANGLE_TO_RAD(gnss->latitude)-ANGLE_TO_RAD(origin.latitude));
    enu->x = EARTH_R*(ANGLE_TO_RAD(gnss->longitude)-ANGLE_TO_RAD(origin.longitude))* cos(ANGLE_TO_RAD(origin.latitude));
}

void target_point_transform()
{
    enu_target_point_len = gnss_target_point_len;
    enu_target_point[0].x = 0;
    enu_target_point[1].y = 0;
    for (int i = 1; i < enu_target_point_len; ++i) {
        gnss_to_enu(gnss_target_point+i, enu_target_point+i);
    }
}

float enu_get_distance(enu_point_t * enu0, enu_point_t * enu1)
{
    return sqrt((enu0->x-enu1->x)*(enu0->x-enu1->x)+(enu0->y-enu1->y)*(enu0->y-enu1->y));
}

float enu_get_angle(enu_point_t * enu0, enu_point_t * enu1)
{
    float angle;
    if (enu0->y == enu1->y)
    {
        if (enu0->x <=enu1->x){
            return 90;
        }
        else{
            return -90;
        }
    }
    else{
        angle = RAD_TO_ANGLE(atan2((enu1->x-enu0->x), (enu1->y-enu0->y)));
    }
    return angle;
}

void enu_go_home()
{
#if 0
    if (remote.protocolState){
        remote.protocolState = 0;
        if (remote.operation == 3 || remote.operation == 4){
            servo_init();
            servo_set_duty(SERVO_MEDIAN);
            position_pid_init(&servo_pid, 15, 0, 9, SERVO_LIMIT, 0);
            *p_target_bldc_duty = 600;
            SCB_CleanInvalidateDCache();
            float yaw, target_angle, angle, distance;
            uint8_t keyNum;
            int16_t duty = 0;
            enu_point_t home;
            home.x = 0;
            home.y = 0;
            while (1){
                enu_get_now_point();
                angle = enu_get_angle(&enu_now_point, &home);
                distance = enu_get_distance(&enu_now_point, &home);

                yaw = convert_angle(eulerAngle.yaw);
                if (yaw < 180)
                    yaw = yaw;
                else
                    yaw = yaw-360;
                target_angle = servo_input_angle(angle,yaw);
                duty = (int16_t)position_pid(&servo_pid, target_angle, 0);
                servo_set_duty(duty+SERVO_MEDIAN);

                if (abs(enu_now_point.x) < 0.5 && abs(enu_now_point.y) < 0.5)
                    break;

                keyNum = get_key_num();
                if (keyNum == BACK){
                    break;
                }
                if (remote.protocolState){
//                    st7789_printf(0, 40, "%d", remote.operation);
                    remote.protocolState = 0;
                    if (remote.operation != 0)
                        break;
                }
            }
            *p_target_bldc_duty = 0;
            SCB_CleanInvalidateDCache();
        }
    }
#endif
}

servo_param_t enu_servo;
uint8_t enu_test_task(uint8_t keyNum)
{
#if 0
    motor_init();
    servo_init();
    servo_set_duty(SERVO_MEDIAN);
    position_pid_init(&servo_pid, 25, 0, 15.0f, SERVO_LIMIT, 0);
    *p_target_bldc_duty = 500;
    SCB_CleanInvalidateDCache();
    uint8_t index = 1;
    float yaw;
    float target_angle;
    float angle;
    float distance = 10;
    int16_t duty = 0;
    bool update_flag = false;
    nrf24l01_enable_tx();
    while (1)
    {
//        enu_get_now_point();
//        angle = enu_get_angle(&enu_now_point, enu_target_point+index);
//        distance = enu_get_distance(&enu_now_point, enu_target_point+index);
        if (gnss_flag == 1){
            gnss_data_parse();
            distance = get_two_points_distance(gnss.latitude, gnss.longitude,gnss_target_point[1].latitude, gnss_target_point[1].longitude);
            angle = get_two_points_azimuth(gnss.latitude, gnss.longitude,gnss_target_point[1].latitude, gnss_target_point[1].longitude);
            gnss_flag = 0;
            update_flag = true;
        }
        yaw = convert_angle(eulerAngle.yaw);
//        if (yaw < 180)
//            yaw = yaw;
//        else
//            yaw = yaw-360;
        target_angle = servo_input_angle(angle,yaw);
        duty = (int16_t)position_pid(&servo_pid, target_angle, 0);
        servo_set_duty(duty+SERVO_MEDIAN);

        st7789_printf(0, 40, "%.3f", angle);
        st7789_printf(0, 60, "%.3f", yaw);
        st7789_printf(0, 80, "%.3f", distance);
        st7789_printf(0, 100, "%3d", duty);
        st7789_printf(0, 120, "%.3f", target_angle);
        if (update_flag){
            if (distance < 3 ){
                break;
            }
            update_flag = false;
        }
//
//        if (abs(enu_now_point.x - enu_target_point[index].x) < 1.5 && abs(enu_now_point.y-enu_target_point[index].y) < 1.5)
//            break;

//        vofa_send_data(enu_target_point[index].x);
//        vofa_send_data(enu_target_point[index].y);
//        vofa_send_data(enu_now_point.x);
//        vofa_send_data(enu_now_point.y);
        vofa_send_data(yaw);
        vofa_send();
        if (remote.protocolState){
            st7789_printf(0, 40, "%d", remote.operation);
            remote.protocolState = 0;
            if (remote.operation != 0)
                break;
        }
        keyNum = get_key_num();
        if (keyNum == BACK){
            break;
        }
    }
    nrf24l01_disable_tx();
    *p_target_bldc_duty = 0;
    SCB_CleanInvalidateDCache();
    motor_stop();
    while(1){
        st7789_printf(0, 40, "%.3f", angle);
        st7789_printf(0, 60, "%.3f", yaw);
        st7789_printf(0, 80, "%.3f", distance);
        st7789_printf(0, 100, "%3d", duty);
        st7789_printf(0, 120, "%.3f", target_angle);
        vofa_send_data(yaw);
        vofa_send();
        if (remote.protocolState){
            st7789_printf(0, 40, "%d", remote.operation);
            remote.protocolState = 0;
            if (remote.operation != 0)
                break;
        }
    }
#endif
    return BACK;
}

#if 0
#define dt          0.002
//#define Q_VALUE     0.001f
//#define R_VALUE     0.001f

arm_matrix_instance_f32 matQ, matR;
//float Q[16] = {0.01f*0.5f*dt*dt, 0,0, 0,
//               0, 0.01f*dt, 0, 0,
//               0, 0, 0.01f*0.5f*dt*dt, 0,
//               0, 0, 0, 0.01f*dt};
float Q[4] = {8, 0.1,
              0.1, 8};
float R[4] = {0.01, 0.001,
              0.001, 0.01};
arm_matrix_instance_f32 matP, matP_;
float P[4] = {1, 0,
               0, 1};
float P_[4] = { 0 };
arm_matrix_instance_f32 matK;
float K[4] = { 0 };
arm_matrix_instance_f32 matZ;
float Z[2] = {0,
              0};
arm_matrix_instance_f32 matX, matX_;
float X[2] = {0,
              0};
float X_[2] = {0,
               0};
arm_matrix_instance_f32 matU;
float U[2] = {0,
              0};
arm_matrix_instance_f32 matA;
float A[4] = {1, 0,
               0, 1};
arm_matrix_instance_f32 matAT;
float AT[4] = {1, 0,
               0, 1};
arm_matrix_instance_f32 matB;
float B[4] = {dt, 0,
              0, dt};
arm_matrix_instance_f32 matH;
float H[4] = {1, 0,
              0, 1};
arm_matrix_instance_f32 matHT;
float HT[4] = {1, 0,
               0, 1};
arm_matrix_instance_f32 matAX_;
float AX_[2] = {0};
arm_matrix_instance_f32 matBU;
float BU[2] = {0};
arm_matrix_instance_f32 matAP;
float AP[4] = {0};
arm_matrix_instance_f32 matAPAT;
float APAT[4] = {0};
arm_matrix_instance_f32 matP_HT;
float P_HT[4] = {0};
arm_matrix_instance_f32 matHP_;
float HP_[4] = {0};
arm_matrix_instance_f32 matHP_HT;
float HP_HT[4] = {0};
arm_matrix_instance_f32 matHP_HT_R;
float HP_HT_R[4] = {0};
arm_matrix_instance_f32 matInvHP_HT_R;
float InvHP_HT_R[4] = {0};
arm_matrix_instance_f32 matHX_;
float HX_[2] = {0};
arm_matrix_instance_f32 mat_HX_;
float _HX_[2] = {0};
arm_matrix_instance_f32 matZ_HX_;
float Z_HX_[2] = {0};
arm_matrix_instance_f32 matKZ_HX_;
float KZ_HX_[2] = {0};
arm_matrix_instance_f32 matI;
float I[4] = {1, 0,
              0, 1};
arm_matrix_instance_f32 matKH;
float KH[4] = {0};
arm_matrix_instance_f32 mat_KH;
float _KH[4] = {0};
arm_matrix_instance_f32 matI_KH;
float I_KH[4] = {0};

gnss_point_t origin;
enu_point_t enu_target_point[FLASH_PAGE_LENGTH/2];
uint16_t enu_target_point_len = 0;

float gnss_vx = 0;
float gnss_vy = 0;

enu_point_t suv_now;
enu_point_t suv_last;
gnss_point_t suv_gnss;
suv_info_t suv_info;

void gnss_to_enu(gnss_point_t * gnss, enu_point_t * enu);

void enu_init()
{
    origin.latitude = gnss_target_point[0].latitude;
    origin.longitude = gnss_target_point[0].longitude;

    // estimate error and measure error
    arm_mat_init_f32(&matQ, 2, 2, Q);
    arm_mat_init_f32(&matR, 2, 2, R);
    // error covariance matrix
    arm_mat_init_f32(&matP_, 2, 2, P_);
    arm_mat_init_f32(&matP, 2, 2, P);
    // kalman gain
    arm_mat_init_f32(&matK, 2, 2, K);
    // measure value
    arm_mat_init_f32(&matZ, 2, 1, Z);
    // measure matrix
    arm_mat_init_f32(&matH, 2, 2, H);
    arm_mat_init_f32(&matHT, 2, 2, HT);
    // estimate value
    arm_mat_init_f32(&matX, 2, 1, X);
    arm_mat_init_f32(&matX_, 2, 1, X_);
    // estimate matrix
    arm_mat_init_f32(&matA, 2, 2, A);
    arm_mat_init_f32(&matAT, 2, 2, AT);
    // control value
    arm_mat_init_f32(&matU, 2, 1, U);
    // control matrix
    arm_mat_init_f32(&matB, 2, 2, B);

    arm_mat_init_f32(&matI, 2, 2, I);

    // calculate the state ahead
    arm_mat_init_f32(&matAX_, 2, 1, AX_);
    arm_mat_init_f32(&matBU, 2, 1, BU);
    // calculate error covariance ahead
    arm_mat_init_f32(&matAP, 2, 2, AP);
    arm_mat_init_f32(&matAPAT, 2, 2, APAT);
    // calculate kalman gain
    arm_mat_init_f32(&matP_HT, 2, 2, P_HT);
    arm_mat_init_f32(&matHP_, 2, 2, HP_);
    arm_mat_init_f32(&matHP_HT, 2, 2, HP_HT);
    arm_mat_init_f32(&matHP_HT_R, 2, 2, HP_HT_R);
    arm_mat_init_f32(&matInvHP_HT_R, 2, 2, InvHP_HT_R);
    // optimal estimate
    arm_mat_init_f32(&matHX_, 2, 1, HX_);
    arm_mat_init_f32(&mat_HX_, 2, 1, _HX_);
    arm_mat_init_f32(&matZ_HX_, 2, 1, Z_HX_);
    arm_mat_init_f32(&matKZ_HX_, 2, 1, KZ_HX_);
    // update error covariance
    arm_mat_init_f32(&matKH, 2, 2, KH);
    arm_mat_init_f32(&mat_KH, 2, 2, _KH);
    arm_mat_init_f32(&matI_KH, 2, 2, I_KH);

    pit_ms_init(PIT_CH12, 2);
    while (gnss_flag == 0){
        __NOP();
        __NOP();
    }
    if (gnss_flag == 1) {
        gnss_data_parse();
        suv_gnss.latitude = gnss.latitude;
        suv_gnss.longitude = gnss.longitude;
//        gnss_vx = gnss.speed * cos(gnss.direction);
//        gnss_vy = gnss.speed * sin(gnss.direction);
        gnss_to_enu(&suv_gnss, &suv_now);
        // update measure matrix
        Z[0] = suv_now.x;
        Z[1] = suv_now.y;
        X[0] = suv_now.x;
        X[1] = suv_now.y;
        suv_info.x_ = suv_now.x;
        suv_info.y_ = suv_now.y;
        gnss_flag = 0;
    }
}

void enu_kalman_update()
{
    // get now position
    if (gnss_flag == 1) {
        gnss_data_parse();
        suv_gnss.latitude = gnss.latitude;
        suv_gnss.longitude = gnss.longitude;
//        gnss_vx = gnss.speed * cos(gnss.direction);
//        gnss_vy = gnss.speed * sin(gnss.direction);
        gnss_to_enu(&suv_gnss, &suv_now);
        // update measure matrix
        Z[0] = suv_now.x;
        Z[1] = suv_now.y;
        gnss_flag = 0;
    }
    // update control matrix
    float yaw = convert_angle(eulerAngle.yaw);
    SCB_CleanInvalidateDCache();
    float speed = *p_bldc_speed;
    U[0] = speed * cos(ANGLE_TO_RAD(yaw));
    U[1] = -speed * sin(ANGLE_TO_RAD(yaw));
    // calculate the state ahead
    arm_mat_mult_f32(&matA, &matX, &matAX_);
    arm_mat_mult_f32(&matB, &matU, &matBU);
    arm_mat_add_f32(&matAX_, &matBU, &matX_);

    // calculate error covariance ahead
    arm_mat_mult_f32(&matA, &matP, &matAP);
    arm_mat_mult_f32(&matAP, &matAT, &matAPAT);
    arm_mat_add_f32(&matAPAT, &matQ, &matP_);

    // calculate kalman gain
    arm_mat_mult_f32(&matP_HT, &matP_, &matP_HT);
    arm_mat_mult_f32(&matH, &matP_, &matHP_);
    arm_mat_mult_f32(&matHP_, &matHT, &matHP_HT);
    arm_mat_add_f32(&matHP_HT, &matR, &matHP_HT_R);
    arm_mat_inverse_f32(&matHP_HT_R, &matInvHP_HT_R);
    arm_mat_mult_f32(&matP_HT, &matInvHP_HT_R, &matK);

    // optimal estimate
    arm_mat_mult_f32(&matH, &matX_, &matHX_);
    arm_mat_scale_f32(&matHX_, -1, &mat_HX_);
    arm_mat_add_f32(&matZ, &mat_HX_, &matZ_HX_);
    arm_mat_mult_f32(&matK, &matZ_HX_, &matKZ_HX_);
    arm_mat_add_f32(&matX_, &matKZ_HX_, &matX);

    // update error covariance
    arm_mat_mult_f32(&matK, &matH, &matKH);
    arm_mat_scale_f32(&matKH, -1, &mat_KH);
    arm_mat_add_f32(&matI, &mat_KH, &matI_KH);
    arm_mat_mult_f32(&matI_KH, &matP_, &matP);

    suv_info.x = X[0];
    suv_info.vx = U[0];
    suv_info.y = X[1];
    suv_info.vy = U[1];
    suv_info.yaw = yaw;
    suv_info.x_ += U[0]*dt;
    suv_info.y_ += U[1]*dt;

//    float ax = imu963ra_data.ax - ax_offset;
//    float ay = imu963ra_data.ay - ay_offset;
//    float az = imu963ra_data.az - az_offset;
//    float pitch = eulerAngle.pitch;
//    float roll = eulerAngle.roll


//    U[0] = speed;

//    U[0] = cos(pitch)* cos(yaw)*ax+ cos(pitch)* sin(yaw)* ay- sin(pitch)*az;
//    U[1] = (sin(roll)* sin(pitch)* cos(yaw) - cos(roll)* sin(yaw))*ax +
//            (sin(roll)* sin(pitch)* sin(yaw) + cos(roll)* cos(yaw))*ay +
//            sin(roll)* cos(pitch)*az;
//    suv_info.vx += U[0]*dt;
//    suv_info.vy += U[1]*dt;
//    if (U[0] < 0.2 && U[0] > -0.2)
//        U[0] = 0;
//    if (U[1] < 0.2 && U[1] > -0.2)
//        U[1] = 0;

//    if (X[1] < 0.2 && X[1] > -0.2){
//        X[1] = 0;
//    }
//    if (X[3] < 0.2 && X[3] > -0.2){
//        X[3] = 0;
//    }

//    // calculate prior covariance matrix(P_(K))
//    P_[0] = P[0] + Q;
//    P_[1] = P[1] + Q;
//    P_[2] = P[2] + Q;
//    P_[3] = P[3] + Q;
//
//    // calculate Kalman gain
//    K[0] = P_[0] / (P_[0] * P_[0] + R);
//    K[1] = 0;
//    K[2] = P_[2] / (P_[2] * P_[2] + R);
//    K[3] = 0;
//
//    // update prior state matrix
//    X_[0] = X[0] + X[1]*dt + 0.5f * U[0] * dt * dt;
//    X_[1] = X[1] + U[0] * dt;
//    X_[2] = X[2] + X[3]*dt + 0.5f * U[1] * dt * dt;
//    X_[3] = X[3] + U[1] * dt;
//
//    // optimal estimate
//    X[0] = X_[0] + K[0] * (Z[0] - X_[0]);
//    X[1] = X_[1];
//    X[2] = X_[2] + K[2] * (Z[1] - X_[2]);
//    X[3] = X_[3];
//
//    // update covariance matrix
//    P[0] = P_[0] * (1 - K[0]);
//    P[1] = P_[1];
//    P[2] = P_[2] * (1 - K[2]);
//    P[3] = P_[3];
//
//    suv_last.x = suv_now.x;
//    suv_last.y = suv_now.y;
//
//    suv_info.x += U[0]*dt;
//    suv_info.vx = U[0];
//    suv_info.y += U[1]*dt;
//    suv_info.vy = U[1];
}
#endif